#include "ros/ros.h"
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <tf/transform_broadcaster.h>
#include "nav_msgs/Odometry.h"

typedef const boost::function<void (const nav_msgs::Odometry::ConstPtr& msg)> Callback;


/**
 * This callback function will build tf message and transform it to "/tf".
 */
void DoOdom(const nav_msgs::Odometry::ConstPtr& msg
    , tf::TransformBroadcaster &tf_pub)
{
    // computing position
    float px = msg->pose.pose.position.x;
    float py = msg->pose.pose.position.y;
    float pz = msg->pose.pose.position.z;
    float rx = msg->pose.pose.orientation.x;
    float ry = msg->pose.pose.orientation.y;
    float rz = msg->pose.pose.orientation.z;
    float rw = msg->pose.pose.orientation.w;

    // publish to tf
    tf_pub.sendTransform(
        tf::StampedTransform(
            tf::Transform(tf::Quaternion(rx,ry,rz,rw), tf::Vector3(px,py,pz)),
            ros::Time::now(),
            "map",
            "odom"));

}


int main(int argc, char **argv)
{
    /**
     * Init Node.
     */
    ros::init(argc, argv, "robot_node");
    ros::NodeHandle n;

    /**
     * Set handler for message.
     */
    tf::TransformBroadcaster tf_pub;
    Callback callback = boost::bind(DoOdom, _1, boost::ref(tf_pub));
    ros::Subscriber sub = n.subscribe("/p3d_odom", 100, callback);
    
    ros::spin();
    
    return 0;
}


